﻿#include "robot.h"

#include "systemconfiguration.h"
#include "../../robothandcommunication/robothandcommunication.h"
#include "component_data.h"
#include "common_log.h"
#include "robot_data.h"

constexpr int g_maxDONum = 32;


Robot::Robot(QObject* parent)
	: ComponentBase(parent)
{
	m_name = u8"机械手组件";
	m_errmsg = "";
	m_type = Component::component_robot;
}

Robot::~Robot()
{}

// 组件初始化处理函数
int Robot::ComponentInit()
{
	auto cfg = syscfg->GetRobotSetting();
	if (cfg.id.isEmpty()) {
		SetErrmsg("机械手编号为空", false);
		return SERIOUS_ERROR;
	}
	m_info.id = cfg.id;
	m_info.brand = cfg.brand;
	m_rob = RobotHandCommunication::GetInstanceByBrand(m_info.brand);
	if (m_rob == nullptr) {
		SetErrmsg(QString("无法创建当前型号(%1)的机械手").arg(m_info.brand), false);
		return SERIOUS_ERROR;
	}
	connect(m_rob, &RobotHandCommunication::ConnectionStateChanged, this, &Robot::OnConnectionStateChanged);
	connect(m_rob, &RobotHandCommunication::RobotHandFaultCode, this, &Robot::OnRobotHandFaultCode);
	connect(m_rob, &RobotHandCommunication::RobotHandDIInfo, this, &Robot::OnRobotHandDIInfo);
	connect(m_rob, &RobotHandCommunication::RobotHandDOInfo, this, &Robot::OnRobotHandDOInfo);
	connect(m_rob, &RobotHandCommunication::RobotHandJoints, this, &Robot::OnRobotHandJoints);
	connect(m_rob, &RobotHandCommunication::RobotHandMoving, this, &Robot::OnRobotHandMoving);
	connect(m_rob, &RobotHandCommunication::RobotHandPointId, this, &Robot::OnRobotHandPointId);
	connect(m_rob, &RobotHandCommunication::RobotHandTargets, this, &Robot::OnRobotHandTargets);
	connect(m_rob, &RobotHandCommunication::RobotHandToolID, this, &Robot::OnRobotHandToolID);
	RobotParam param = {};
	param.id = m_info.id;
	param.ip = cfg.ip;
	param.statusPort = cfg.statePort;
	param.cmdPort = cfg.cmdPort;
	param.base = cfg.origin;
	int ret = m_rob->Init(param);
	if (ret == 0) {
		InitRobotState(true);
	} else if (ret == -2) {
		SetErrmsg("命令端口连接失败", false);
		InitRobotState(false);
	} else if (ret == -3) {
		SetErrmsg("状态端口连接失败", false);
		InitRobotState(false);
	} else {
		log_error("ip = {}, stateport = {}, cmdport = {}", param.ip, param.statusPort, param.cmdPort);
		QString str = "机械手初始化错误：" + (ret == -1) ? "配置参数错误" : "未知错误";
		SetErrmsg(str, false);
		return SERIOUS_ERROR;
	}
	m_info.origin = cfg.origin;
	
	return 0;
}
/******************************信息获取接口***************************************/
/*获取机械手当前是否停止运行*/
bool Robot::GetStopMoveState()
{
	return m_state.run == RunState::robot_state_normal;
}

/*获取机械手的当前坐标*/
Position Robot::GetRobotCurrentPos()
{
	Position out = {};
	if (!m_state.target.isEmpty()) {
		PositionJ pos = {};
		pos.fromJson(m_state.target.toUtf8());
		out.TransFromPositonJ(pos);
	}
	return out;
}

RobotInfo Robot::GetRobotStateInfo()
{
	return m_state;
}

/************************************组件接口******************************************/
/*自检接口*/
bool Robot::ComponentCheckSelfState()
{
	if (!m_state.connected) {
		SetErrmsg("机械手网络未连接", false);
		return false;
	}
	if (m_state.faultcode != 0) {
		SetErrmsg(QString("机械手本体发生故障, 故障代码：%1, 故障描述：%2").
			arg(m_state.faultcode).arg(m_state.faultMsg), false);
		return false;
	}
	return true;
}
/*复位接口*/
bool Robot::ComponentSelfReset()
{
	log_trace("robot reset");
	if (!m_state.connected) {
		QByteArray str;
		if (!Reconnecte(str)) {
			SetErrmsg(str, false);
			return false;
		}
	}
	if (m_state.run == RunState::robot_state_stop) {
		log_error("robot is stop");
	}
	if (m_state.faultcode != 0) {
		SetErrmsg(QString("机械手本体发生故障, 故障代码：%1, 故障描述：%2").arg(m_state.faultcode).arg(m_state.faultMsg));
		return false;
	}
	InitRobotState(true);
	return true;
}
/*系统状态响应接口*/
void Robot::ResponseSystemState(int state)
{
	/*响应系统停止*/
	if (state == (int)SystemStateType::system_state_stop) {
		//m_robot->Stop();
		Pause();
		log_trace("system is stop, then rob stop");
	}

	if (state == (int)SystemStateType::system_state_work) {
		Resum();
		log_trace("system is work, then rob work");
	}
}
// 注册订阅函数接口
bool Robot::RegisterSubscriberFunction(int type, Callback callback)
{
	if (callback == nullptr) {
		log_error("callback function is null");
		return false;
	}

	if (type <= 0 || type >= (int)RobotMsgType::robot_msg_max) {
		log_error("msg type error, type ={}", type);
		return false;
	}

	auto msg = (RobotMsgType)type;
	if (m_subscribers.find(msg) == m_subscribers.end()) {
		QVector<Callback> temp;
		temp.append(callback);
		m_subscribers.insert(msg, temp);
	} else {
		m_subscribers[msg].push_back(callback);
	}
	return true;
}
// 命令执行接口
bool Robot::ExcuteCmd(int cmd, const QByteArray& input, QByteArray& output)
{
	bool ret = false;
	RobotCmdType type = (RobotCmdType)cmd;
	switch (type) {
	case RobotCmdType::robot_cmd_get_connect:
		if (m_state.connected) {
			output = "已连接";
		} else {
			output = "未连接";
		}
		ret = true;
		break;
	case RobotCmdType::robot_cmd_get_target:
		output = m_state.target.toUtf8();
		ret = true;
		break;
	case RobotCmdType::robot_cmd_get_joints:
		output = m_state.joints.toUtf8();
		ret = true;
		break;
	case RobotCmdType::robot_cmd_get_di:
	{
		int index = input.toInt();
		if (index <= 0 || index > g_maxDONum) {
			output = m_state.diInfo.toUtf8();
		} else {
			output = m_state.diInfo.mid(index, 1).toUtf8();
		}
	}
	ret = true;
	break;
	case RobotCmdType::robot_cmd_reconnected:
		return Reconnecte(output);
		break;
	case RobotCmdType::robot_cmd_set_do:
		if (SetDO(input)) {
			output = "设置成功";
			return true;
		} else {
			output = "设置失败";
			return true;
		}
		break;
	case RobotCmdType::robot_cmd_set_grip:
		if (SetGrip(input)) {
			output = "设置成功";
			return true;
		} else {
			output = "设置失败";
			return true;
		}
		break;
	case RobotCmdType::robot_cmd_set_move:
		if (MoveJL(input)) {
			output = "移动指令下发成功";
			return true;
		} else {
			output = "移动指令下发失败";
			return true;
		}
		break;
	default:
		output = "当前命令类型不支持";
		break;
	}

	return ret;
}

/*重连机械手*/
bool Robot::Reconnecte(QByteArray& out)
{
	int ret = m_rob->Reconnecte();
	if (ret == 0) {
		m_state.connected = 0;
		out = "连接成功";
		return true;
	}
	switch (ret) {
	case -1:
		out = "状态端口连接失败";
		break;
	case -2:
		out = "命令端口连接失败";
		break;
	default:
		out = "连接失败";
		break;
	}
	return false;
}
/*暂停机械手*/
bool Robot::Pause()
{
	if (m_rob->Pause()) {
		log_trace("robot is pause");
		m_state.run = RunState::robot_state_pause;
		return true;
	}
	log_trace("Set robot pause failed");
	return false;
}
/*恢复机械手*/
bool Robot::Resum()
{
	if (m_rob->Resume()) {
		log_trace("robot is resume");
		m_state.run = RunState::robot_state_normal;
		return true;
	}
	log_trace("Set robot resume failed");
	return false;
}
/*停止机械手*/
bool Robot::Stop()
{
	if (m_rob->Stop()) {
		log_trace("robot is stop");
		m_state.run = RunState::robot_state_stop;
		return true;
	}
	log_trace("Set robot stop failed");
	return false;
}

/*设置DO*/
bool Robot::SetDO(int index, bool val)
{
	if (index > g_maxDONum || index <= 0) {
		log_error("Set DO  index param {} is error", index);
		return false;
	}
	log_trace("Set do index {} to {}", index, val);
	return m_rob->SetDo(index, val);
}
bool Robot::SetDO(const QByteArray& input)
{
	SetDOParam param = {};
	param.fromJson(input);
	return SetDO(param.index(), param.val() == 1 ? true : false);
}
/*复合指令*/
bool Robot::SetGrip(int index)
{
	log_trace("Set grip index = {}", index);
	return m_rob->SetGrip(index);
}
bool Robot::SetGrip(const QByteArray& input)
{
	GripParam param = {};
	param.fromJson(input);
	return SetGrip(param.index());
}

bool Robot::MoveJL(const QList<MoveJLInfo>& moves)
{
	auto& point1 = moves.at(0);
	if (CheckPoints(point1.pos, 10.0f)) {
		return m_rob->MoveJL(moves);
	} else {
		log_error("the difference between path first point and current pos is too large");
		log_error("下发路径的起始点与机械手的当前坐标差距过大，路径无法执行");
		return false;
	}
}
bool Robot::MoveJL(const QByteArray& input)
{
	QString str = input;
	if (str == "origin") {
		QList<MoveJLInfo> moves;
		MoveJLInfo move0 = {};
		move0.acc = 20;
		move0.id = 1;
		move0.di = -1;
		move0.vel = 20;
		move0.pointType = 2;
		move0.zone = 10;
		move0.name = str;
		move0.pos = GetRobotCurrentPos();
		moves.append(move0);

		MoveJLInfo move = {};
		move.acc = 20;
		move.id = 2;
		move.di = -1;
		move.vel = 20;
		move.pointType = 2;
		move.zone = 10;
		move.name = str;
		move.pos = m_info.origin;
		moves.append(move);
		return MoveJL(moves);
	}

	return false;
}


void Robot::InitRobotState(bool connected)
{
	m_state.connected = connected;
	m_state.diInfo.clear();
	m_state.doinfo.clear();
	m_state.faultcode = 0;
	m_state.faultMsg.clear();
	m_state.joints.clear();
	m_state.target.clear();
	m_state.moving = 0;
	if (connected) {
		m_state.run = RunState::robot_state_normal;
	} else {
		m_state.run = RunState::robot_state_stop;
	}
}

/*校验坐标和机械手的坐标是否一致*/
bool Robot::CheckPoints(const Position& pos, float rang)
{
	auto cur = GetRobotCurrentPos();
	if (std::abs(cur.x - pos.x) > rang) {
		log_error("cur.x {}, pos.x{}", cur.x, pos.x);
		return false;
	}
	if (std::abs(cur.y - pos.y) > rang) {
		log_error("cur.y {}, pos.y{}", cur.y, pos.y);
		return false;
	}
	if (std::abs(cur.z - pos.z) > rang) {
		log_error("cur.z {}, pos.z{}", cur.z, pos.z);
		return false;
	}
	if (std::abs(cur.a - 180) < 1.0f || std::abs(cur.a + 180) < 1.0f) {
		if (std::abs(cur.a - 180) > 10.0f && std::abs(cur.a + 180) > 10.0f) {
			return false;
		}
	} else {
		if (std::abs(cur.a - pos.a) > 10.0f) {
			log_error("cur.a {}, pos.a{}", cur.a, pos.a);
			return false;
		}
	}

	return true;
}


/********************************************机械手状态槽函数*************************************************/
// 连接状态更改信号, 0:连接 -1：状态断开断开 -2：连接端口端口 -3：同时断开
void Robot::OnConnectionStateChanged(int state)
{
	if (state != 0) {
		SetErrmsg("机械手网络断开连接");
		m_state.connected = false;
	} else {
		m_state.connected = true;
	}
}
// 执行点位ID信号
void Robot::OnRobotHandPointId(int id)
{
	if (id == 0) {
		return;
	}
	QByteArray data;
	data = QString::number(id).toUtf8();
	auto callbacks = m_subscribers.value(RobotMsgType::robot_msg_point_id);
	for (auto& call : callbacks) {
		call(data);
	}
}
// TCP坐标信号
void Robot::OnRobotHandJoints(const QString& joints)
{
	QByteArray data = joints.toLocal8Bit();
	auto callbacks = m_subscribers.value(RobotMsgType::robot_msg_joints);
	for (auto& call : callbacks) {
		call(data);
	}
	m_state.joints = joints;
}
// 关节角度值信号
void Robot::OnRobotHandTargets(const QString& targets)
{
	QByteArray data = targets.toLocal8Bit();
	auto callbacks = m_subscribers.value(RobotMsgType::robot_msg_target);
	for (auto& call : callbacks) {
		call(data);
	}
	m_state.target = targets;
}
// 运动状态信息 0: 静止 1：运动
void Robot::OnRobotHandMoving(int flag)
{
	m_state.moving = flag == 1 ? true : false;
}
// 夹具编号
void Robot::OnRobotHandToolID(int id)
{
	m_state.toolid = id;
}
// 错误码信号
void Robot::OnRobotHandFaultCode(int code, const QString& err)
{
	if (m_state.faultcode == code) {
		return;
	}
	SetErrmsg(QString("机械手本体发生故障, 故障代码：%1, 故障描述：%2").arg(code).arg(err));
	m_state.faultcode = code;
	m_state.faultMsg = err;
}
// DI信息
void Robot::OnRobotHandDIInfo(const QString& diInfo)
{
	m_state.diInfo = diInfo;
	QByteArray data;
	data.append(diInfo.toUtf8());
	auto callbacks = m_subscribers.value(RobotMsgType::robot_msg_DI);
	for (auto& call : callbacks) {
		call(data);
	}
}
// DO信息
void Robot::OnRobotHandDOInfo(const QString& doInfo)
{
	m_state.doinfo = doInfo;
	QByteArray data;
	data.append(doInfo.toUtf8());
	auto callbacks = m_subscribers.value(RobotMsgType::robot_msg_DO);
	for (auto& call : callbacks) {
		call(data);
	}
}
